#pragma once

#include <array>
#include "rtdev/safetyboard/scbitf.h"
#include "rtdev/safetyboard/cab.h"
#include "rtdev/safetyboard/iodev.h"
#include "rtdev/safetyboard/powerdev.h"

namespace rtdev {
class RoboHubScb;

/**
 * @brief Robohub本体电源监控,有两路电源
 */
class HubRobotPower : public ScbRobPowerItf
{
public:
    HubRobotPower();
    int set_power(bool power, int robid = -1) override;
    double get_power(int robid) override;
    double get_voltage(int robid) override;
    double get_current(int robid) override;

    struct Feedback
    {
        bool is_cmd_power_on = false;
        double voltage = 0;
        double current = 0;
        double power = 0;
    };
    std::array<Feedback, 2> feedbacks;
    RoboHubScb* scb;
};
class RoboHubCab : public rtdev::DefaultCab
{
public:
    RoboHubCab();

    int get_id();
    int set_id(uint32_t id);
    int get_stuck_voltage(bool is_act);
    int set_stuck_voltage(int voltage);
    int set_power_supply(int val);
    int get_power_supply();

    uint16_t cmd_stuck_voltage, act_stuck_voltage;
    int32_t cmd_cab_id, act_cab_id;
};

class RoboHubSafetySignal : public rtdev::ScbSafetySignalItf
{
public:
    RoboHubSafetySignal();
    int get_ei_stat(bool dual_chn[2], bool* has_fault) override;
    int get_ext_ei_stat(bool dual_chn[2], bool* has_fault) override;
    int get_si_stat(bool dual_chn[2], bool* has_fault) override;
    int set_tcp_offset(float x, float y, float z) override;

    bool ei[2], ei_fault;
    bool ext_ei[2], ext_ei_fault;
    bool si[2], si_fault;
};

class RoboHubScb : public SafetyBoardItf
{
public:
    RoboHubScb();
    const char* name() override;
    int init(void* cfginfo, int sim) override;
    int boot() override;

    ScbRobPowerItf* rob_power() override { return &rob; }
    rtdev::ScbCabItf* cab() override { return &cab_; }
    rtdev::ScbSafetySignalItf* safety_signal() override { return &safedev; }
    rtdev::ScbIOItf* io() override { return &iodev; }

    int set_send_data(int busid) override;
    int get_recv_data(int busid) override;

    int get_sdo_blocking(uint32_t objid, void* data, int s, int retry = 3) override;
    int set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback = nullptr, int retry = 3) override;

private:
    rtdev::RoboHubSafetySignal safedev;
    rtdev::NoIoDev iodev;
    HubRobotPower rob;
    RoboHubCab cab_;
    std::shared_ptr<rtbus::RtCommuItf> bus_;
    std::shared_ptr<rtbus::RtCommuItf> bus2_;
    int bus_id_, bus2_id_;

private:
    uint32_t last_sword;
};

}  // namespace rtdev
